//------------------------------------------------------------------------------
//  <copyright file="SDFTests.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
//  </copyright>
//------------------------------------------------------------------------------
namespace Test.Robotics.Geometry
{
    using System;
    using System.Collections.Generic;
    using System.Linq;
    using System.Text;
    using System.Threading.Tasks;

    using Microsoft.Robotics.Geometry;
    using Microsoft.Robotics.Geometry.SDF;
    using Microsoft.Robotics.Numerics;
    using Microsoft.VisualStudio.TestTools.UnitTesting;
    
    /// <summary>
    /// Test the <see cref="SDFTests"/> class.
    /// </summary>
    [TestClass]
    public class SDFTests
    {
        /// <summary>
        /// Test the constructor for SDF
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void SDFConstructorTest()
        {
            Vector3 dim = new Vector3(1, 1.5, 2);
            double resolution = 0.01;

            SDF sdf = new SDF(dim, resolution);

            Assert.AreEqual(sdf.Width, (int)(dim.X / resolution));
            Assert.AreEqual(sdf.Height, (int)(dim.Y / resolution));
            Assert.AreEqual(sdf.Depth, (int)(dim.Z / resolution));
            Assert.AreEqual(sdf.Resolution, resolution);
        }

        /// <summary>
        /// Tests the indexing into the SDF
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void SDFIndexingTest()
        {
            SDF sdf = new SDF(100, 100, 100, 0.01);
            sdf[50, 50, 50] = 5.0;

            Assert.AreEqual(sdf[50, 50, 50], 5.0);
        }

        /// <summary>
        /// Tests the conversion from grid point to 3D
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void SDFGridTo3D()
        {
            int width = 100;
            int height = 150;
            int depth = 200;
            double resolution = 0.01;

            SDF sdf = new SDF(width, height, depth, resolution);

            Vector3 point = sdf.Get3DPoint(50, 50, 50);

            Assert.AreEqual(point.X, (double)(50 - width / 2) * resolution);
            Assert.AreEqual(point.Y, (double)(50 - height / 2) * resolution);
            Assert.AreEqual(point.Z, (double)(50 - depth / 2) * resolution);
        }

        /// <summary>
        /// Tests the conversion from 3D point to grid point
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void SDF3DToGrid()
        {
            int width = 100;
            int height = 150;
            int depth = 200;
            double resolution = 0.01;

            SDF sdf = new SDF(width, height, depth, resolution);

            Vector3 point = sdf.GetGridPoint(new Vector3(0, 0, 0));

            Assert.AreEqual(point.X, width / 2);
            Assert.AreEqual(point.Y, height / 2);
            Assert.AreEqual(point.Z,  depth / 2);
        }

        /// <summary>
        /// Tests the interpolation
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void SDFInterpolate()
        {
            int width = 100;
            int height = 150;
            int depth = 200;
            double resolution = 0.01;

            SDF sdf = new SDF(width, height, depth, resolution);

            sdf[10, 10, 10] = 1.0;
            sdf[10, 10, 11] = 2.0;
            sdf[10, 11, 10] = 1.0;
            sdf[10, 11, 11] = 2.0;

            sdf[11, 10, 10] = 1.0;
            sdf[11, 10, 11] = 2.0;
            sdf[11, 11, 10] = 1.0;
            sdf[11, 11, 11] = 2.0;

            Vector3 point = sdf.Get3DPoint(10, 10, 10) + 0.5 * (sdf.Get3DPoint(11, 11, 11) - sdf.Get3DPoint(10, 10, 10));
            Vector3 gridPoint = sdf.GetGridPoint(point);
            
            double val1 = sdf.Interpolate(gridPoint);
            double val2 = sdf.Eval(point);

            Assert.AreEqual(val1, 1.5);
            Assert.AreEqual(val2, 1.5);
        }

        /// <summary>
        /// Tests the interpolation
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void SDFGradient()
        {
            int width = 100;
            int height = 150;
            int depth = 200;
            double resolution = 0.01;

            SDF sdf = new SDF(width, height, depth, resolution);

            sdf[10, 10, 10] = 1.0;
            sdf[10, 10, 11] = 2.0;
            sdf[10, 11, 10] = 1.0;
            sdf[10, 11, 11] = 2.0;

            sdf[11, 10, 10] = 1.0;
            sdf[11, 10, 11] = 2.0;
            sdf[11, 11, 10] = 1.0;
            sdf[11, 11, 11] = 2.0;

            Vector3 point = sdf.Get3DPoint(10, 10, 10) + 0.5 * (sdf.Get3DPoint(11, 11, 11) - sdf.Get3DPoint(10, 10, 10));
            Vector3 gradient = sdf.Gradient(point);

            Assert.AreEqual(gradient.X, 0);
            Assert.AreEqual(gradient.Y, 0);
            Assert.AreEqual(Math.Round(gradient.Z, 4), 1);
        }
    }
}
